/*
 * Copyright (c) 2009, Hamish Morgan. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * 3. Neither the name of the University of Sussex nor the names of its
 *    contributors may be used to endorse or promote products derived from
 *    this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */
package locusts.lib;

import javax.vecmath.Vector2d;
import javax.vecmath.Vector2d;

/**
 * Utility class to hold a number of static maths functions.
 * 
 * @author Hamish Morgan
 */
public class MathUtil {

    public static final double PI = Math.PI;
    public static final double TWO_PI = 2d * PI;
    public static final double HALF_PI = 0.5d * PI;

    private MathUtil() {
    }

    /**
     * Normalise the angle a: so it is in the range: 0 <= a < 2*pi
     * 
     * @param angle
     * @return
     */
    public static double normaliseAnglePositive(final double angle) {
        if (angle < 0d)
            return TWO_PI + (angle % TWO_PI);
        else
            return (angle % TWO_PI);
    }

    /**
     * Normalise the angle a: so it is in the range: -pi <= a < pi
     * 
     * @param angle
     * @return
     */
    public static double normaliseAngle(final double angle) {
        final double trimmedAngle = angle % TWO_PI;
        if (trimmedAngle >= PI)
            return (trimmedAngle % PI) - PI;
        else if (trimmedAngle < -PI)
            return PI + (trimmedAngle % PI);
        else
            return trimmedAngle;
    }

    /**
     * Return the angle in radians between the 2d vectors given in the
     * parameters.
     * 
     * @param ax
     * @param ay
     * @param bx
     * @param by
     * @return
     */
    public static double angleBetween(double ax, double ay, double bx,
            double by) {
        return normaliseAngle(Math.atan2(ay, ax) - Math.atan2(by, bx));

        // alternative that is slower 'as-is' but may be faster in some cases
        // when a comparison is needed and cos(theta) can be precalculated:
        //
        // return normalise(Math.acos(a.dot(b) / (a.length() * b.length())));
    }

    /**
     * Return the angle in radians between the 2d vectors given in the
     * parameters.
     * 
     * @param a
     * @param b
     * @return
     */
    public static double angleBetween(Vector2d a, Vector2d b) {
        return angleBetween(a.x, a.y, b.x, b.y);
    }

    /**
     * 
     * @param v1
     * @param v2
     * @param epsilon
     * @return
     */
    public static final boolean epsilonEqual(final double v1,
            final double v2,
            final double epsilon) {
        final double diff = v1 - v2;
        return diff < epsilon && diff > 0d - epsilon;
    }

    public static final Vector2d max(final Vector2d[] vecs) {
        return vecs[argmax(vecs)];
    }

    public static final Vector2d min(final Vector2d[] vecs) {
        return vecs[argmin(vecs)];
    }

    public static final int argmax(final Vector2d[] vecs) {
        if (vecs == null || vecs.length == 0)
            throw new IllegalArgumentException();

        int argmax = 0;
        double maxLen = vecs[0].lengthSquared();

        for (int i = 1; i < vecs.length; i++) {
            final double len = vecs[i].lengthSquared();
            if (len > maxLen) {
                maxLen = len;
                argmax = i;
            }
        }
        return argmax;
    }

    /**
     * Return the index of the vector in the array that has the
     * smallest magnitude.
     * 
     * @param vecs
     * @return
     */
    public static final int argmin(final Vector2d[] vecs) {
        if (vecs == null || vecs.length == 0)
            throw new IllegalArgumentException();

        int argmin = 0;
        double minLen = vecs[0].lengthSquared();

        for (int i = 1; i < vecs.length; i++) {
            final double len = vecs[i].lengthSquared();
            if (len < minLen) {
                minLen = len;
                argmin = i;
            }
        }
        return argmin;
    }

 




    /**
     * Calculate the mean average vector.
     * @param vecs
     * @return
     */
    public static final Vector2d average(final Vector2d[] vecs) {
        if (vecs == null || vecs.length == 0)
            throw new IllegalArgumentException();

        final Vector2d out = sum(vecs);

        out.scale(1d / vecs.length);

        return out;
    }

    /**
     * Add the vectors int he array together.
     * 
     * @param vecs
     * @return
     */
    public static final Vector2d sum(final Vector2d[] vecs) {
        if (vecs == null)
            throw new IllegalArgumentException();

        final Vector2d out = new Vector2d();

        for (int i = 0; i < vecs.length; i++)
            out.add(vecs[i]);

        return out;
    }


}
